//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
//
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//
//  $File: SimulatedHexapod.cs $ $Revision: 1 $
//-----------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;

using System;
using System.Collections.Generic;

using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
using System.ComponentModel;
using xna = Microsoft.Xna.Framework;
using xnagrfx = Microsoft.Xna.Framework.Graphics;

namespace ProMRDS.Simulation.Hexapod
{
    [DisplayName("Simulated Hexapod")]
    [Description("Simulated Hexapod Service")]
    [Contract(Contract.Identifier)]
    public class HexapodEntityService : DsspServiceBase
    {
        [Partner("Engine", 
            Contract = engineproxy.Contract.Identifier, 
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate)]
        private engineproxy.SimulationEnginePort _engineStub = 
            new engineproxy.SimulationEnginePort();

        // Main service port
        [ServicePort("/SimulatedHexapod", AllowMultipleInstances=false)]
        private SimulatedHexapodOperations _mainPort = 
            new SimulatedHexapodOperations();

        public HexapodEntityService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {

        }

        protected override void Start()
        {
            base.Start();
            // Orient sim camera view point
            SetupCamera();
            // Add objects (entities) in our simulated world
            PopulateWorld();
        }

        private void SetupCamera()
        {
            // Set up initial view
            CameraView view = new CameraView();
            view.EyePosition = new Vector3(19.83f, -3.55f, -7.33f);
            view.LookAtPoint = new Vector3(20.24f, -3.67f, -6.96f);
            SimulationEngine.GlobalInstancePort.Update(view);
        }

        private void PopulateWorld()
        {
            AddSky();
            AddGround();

            HexapodEntity hexapod1 = CreateHexapod("Hexapod1", new Vector3(20.52f, -3.97f, -5.94f));
            hexapod1.Rotation = new xna.Vector3(-2.49f, 14.75f, -6.42f);
            SimulationEngine.GlobalInstancePort.Insert(hexapod1);

            HexapodEntity hexapod2 = CreateHexapod("Hexapod2", new Vector3(21.29f, -4.19f, -6.5f));
            hexapod1.Rotation = new xna.Vector3(4.21f, 47.34f, -3.85f);
            SimulationEngine.GlobalInstancePort.Insert(hexapod2);
        }

        public struct HexapodShapeDescriptor
        {
            public Shapes ShapeID;
            public string Name;
            public double xPosition;
            public double yPosition;
            public double zPosition;
            public double xSize;
            public double ySize;
            public double zSize;
            public double radius;
            public double xRotation;
            public double yRotation;
            public double zRotation;
            public double mass;
            public string mesh;
            public HexapodShapeDescriptor(
                Shapes _ShapeID,
                string _Name,
                double _xPosition,
                double _yPosition,
                double _zPosition,
                double _xSize,
                double _ySize,
                double _zSize,
                double _radius,
                double _xRotation,
                double _yRotation,
                double _zRotation,
                double _mass,
                string _mesh)
            {
                ShapeID = _ShapeID;
                Name = _Name;
                xPosition = _xPosition;
                yPosition = _yPosition;
                zPosition = _zPosition;
                xSize = _xSize;
                ySize = _ySize;
                zSize = _zSize;
                radius = _radius;
                xRotation = _xRotation;
                yRotation = _yRotation;
                zRotation = _zRotation;
                mass = _mass;
                mesh = _mesh;
            }
        }

        public class ParentChild
        {
            public string Parent;
            public string Child;
            public string JointName;
            public Vector3 ChildConnect;
            public Vector3 ParentConnect;
            public Vector3 ParentNormal;
            public Vector3 ParentAxis;
            public Vector3 ChildNormal;
            public Vector3 ChildAxis;
            public ParentChild(string child, string parent, string jointName, Vector3 childConnect, Vector3 parentConnect, Vector3 childNormal, Vector3 childAxis, Vector3 parentNormal, Vector3 parentAxis)
            {
                Child = child;
                Parent = parent;
                JointName = jointName;
                ChildConnect = childConnect;
                ParentConnect = parentConnect;
                ParentNormal = parentNormal;
                ParentAxis = parentAxis;
                ChildNormal = childNormal;
                ChildAxis = childAxis;
            }
        }
        
        // units are in meters
        // Adjustable parameters:
        const float _bodyLength = 0.3f;
        const float _bodyWidth = 0.2f;
        const float _bodyHeight = 0.08f;
        const float _upperLegRadius = 0.03f;
        const float _lowerLegRadius = 0.025f;
        const float _upperLegLength = 0.1f;
        const float _lowerLegLength = 0.15f;

        HexapodShapeDescriptor[] ShapeDescriptors = new HexapodShapeDescriptor[]
        {
            new HexapodShapeDescriptor(Shapes.Box, "Hexapod", 0, _upperLegLength + _lowerLegLength, 0, _bodyWidth, _bodyHeight, _bodyLength, 0, 0, 0, 0, 1, "HexapodBody.obj"),
            new HexapodShapeDescriptor(Shapes.Sphere, "FRShoulder", _bodyWidth/2 + _upperLegRadius * 2, _bodyWidth/2 + _upperLegRadius, _bodyLength/2 - _upperLegRadius, 0, 0, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodShoulder.obj"),
            new HexapodShapeDescriptor(Shapes.Capsule, "FRUpper", _bodyWidth/2 + _upperLegRadius * 5, _upperLegLength/2 + _lowerLegLength + _lowerLegRadius, _bodyLength/2 - _upperLegRadius, 0, _upperLegLength, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodUpperLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Box, "FRLower", _bodyWidth/2 + _upperLegRadius * 5, _lowerLegLength/2 + _lowerLegRadius, _bodyLength/2 - _upperLegRadius, _lowerLegRadius*2, _lowerLegLength + _lowerLegRadius*2, _lowerLegRadius*2, _lowerLegRadius, 0, 0, 0, 1, "HexapodLowerLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Sphere, "MRShoulder", _bodyWidth/2 + _upperLegRadius * 2, _bodyWidth/2 + _upperLegRadius, 0, 0, 0, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodShoulder.obj"),
            new HexapodShapeDescriptor(Shapes.Capsule, "MRUpper", _bodyWidth/2 + _upperLegRadius * 5, _upperLegLength/2 + _lowerLegLength + _lowerLegRadius, 0, 0, _upperLegLength, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodUpperLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Box, "MRLower", _bodyWidth/2 + _upperLegRadius * 5, _lowerLegLength/2 + _lowerLegRadius, 0, _lowerLegRadius*2, _lowerLegLength + _lowerLegRadius*2, _lowerLegRadius*2, _lowerLegRadius, 0, 0, 0, 1, "HexapodLowerLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Sphere, "RRShoulder", _bodyWidth/2 + _upperLegRadius * 2, _bodyWidth/2 + _upperLegRadius, -_bodyLength/2 + _upperLegRadius, 0, 0, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodShoulder.obj"),
            new HexapodShapeDescriptor(Shapes.Capsule, "RRUpper", _bodyWidth/2 + _upperLegRadius * 5, _upperLegLength/2 + _lowerLegLength + _lowerLegRadius, -_bodyLength/2 + _upperLegRadius, 0, _upperLegLength, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodUpperLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Box, "RRLower", _bodyWidth/2 + _upperLegRadius * 5, _lowerLegLength/2 + _lowerLegRadius, -_bodyLength/2 + _upperLegRadius, _lowerLegRadius*2, _lowerLegLength + _lowerLegRadius*2, _lowerLegRadius*2, _lowerLegRadius, 0, 0, 0, 1, "HexapodLowerLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Sphere, "FLShoulder", -(_bodyWidth/2 + _upperLegRadius * 2), _bodyWidth/2 + _upperLegRadius, _bodyLength/2 - _upperLegRadius, 0, 0, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodShoulder.obj"),
            new HexapodShapeDescriptor(Shapes.Capsule, "FLUpper", -(_bodyWidth/2 + _upperLegRadius * 5), _upperLegLength/2 + _lowerLegLength + _lowerLegRadius, _bodyLength/2 - _upperLegRadius, 0, _upperLegLength, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodUpperLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Box, "FLLower", -(_bodyWidth/2 + _upperLegRadius * 5), _lowerLegLength/2 + _lowerLegRadius, _bodyLength/2 - _upperLegRadius, _lowerLegRadius*2, _lowerLegLength + _lowerLegRadius*2, _lowerLegRadius*2, _lowerLegRadius, 0, 0, 0, 1, "HexapodLowerLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Sphere, "MLShoulder", -(_bodyWidth/2 + _upperLegRadius * 2), _bodyWidth/2 + _upperLegRadius, 0, 0, 0, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodShoulder.obj"),
            new HexapodShapeDescriptor(Shapes.Capsule, "MLUpper", -(_bodyWidth/2 + _upperLegRadius * 5), _upperLegLength/2 + _lowerLegLength + _lowerLegRadius, 0, 0, _upperLegLength, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodUpperLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Box, "MLLower", -(_bodyWidth/2 + _upperLegRadius * 5), _lowerLegLength/2 + _lowerLegRadius, 0, _lowerLegRadius*2, _lowerLegLength + _lowerLegRadius*2, _lowerLegRadius*2, _lowerLegRadius, 0, 0, 0, 1, "HexapodLowerLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Sphere, "RLShoulder", -(_bodyWidth/2 + _upperLegRadius * 2), _bodyWidth/2 + _upperLegRadius, -_bodyLength/2 + _upperLegRadius, 0, 0, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodShoulder.obj"),
            new HexapodShapeDescriptor(Shapes.Capsule, "RLUpper", -(_bodyWidth/2 + _upperLegRadius * 5), _upperLegLength/2 + _lowerLegLength + _lowerLegRadius, -_bodyLength/2 + _upperLegRadius, 0, _upperLegLength, 0, _upperLegRadius, 0, 0, 0, 1, "HexapodUpperLeg.obj"),
            new HexapodShapeDescriptor(Shapes.Box, "RLLower", -(_bodyWidth/2 + _upperLegRadius * 5), _lowerLegLength/2 + _lowerLegRadius, -_bodyLength/2 + _upperLegRadius, _lowerLegRadius*2, _lowerLegLength + _lowerLegRadius*2, _lowerLegRadius*2, _lowerLegRadius, 0, 0, 0, 1, "HexapodLowerLeg.obj"),
        };

        static Vector3 X = new Vector3(1, 0, 0);
        static Vector3 Y = new Vector3(0, 1, 0);
        static Vector3 Z = new Vector3(0, 0, 1);
        static Vector3 nX = new Vector3(-1, 0, 0);
        static Vector3 nY = new Vector3(0, -1, 0);
        static Vector3 nZ = new Vector3(0, 0, -1);

        ParentChild[] Relationships = new ParentChild[]
        {
            new ParentChild("FRShoulder", "Hexapod", "FRShoulder", new Vector3(0, 0, _upperLegRadius), new Vector3(_bodyWidth/2, 0, _bodyLength/2 - _upperLegRadius), Y, X, -X, Y),
            new ParentChild("FRUpper", "FRShoulder", "FRShoulder2", new Vector3(0, _upperLegLength/2, 0), new Vector3(0,0,0),Y, X, Z, -Y),
            new ParentChild("FRLower", "FRUpper", "FRElbow", new Vector3(0, _lowerLegLength/2, 0), new Vector3(0,-_upperLegLength/2,0),Y, X, Y, X),
            new ParentChild("MRShoulder", "Hexapod", "MRShoulder", new Vector3(0, 0, _upperLegRadius), new Vector3(_bodyWidth/2, 0, 0), Y, X, -X, Y),
            new ParentChild("MRUpper", "MRShoulder", "MRShoulder2", new Vector3(0, _upperLegLength/2, 0), new Vector3(0,0,0),Y, X, Z, -Y),
            new ParentChild("MRLower", "MRUpper", "MRElbow", new Vector3(0, _lowerLegLength/2, 0), new Vector3(0,-_upperLegLength/2,0),Y, X, Y, X),
            new ParentChild("RRShoulder", "Hexapod", "RRShoulder", new Vector3(0, 0, _upperLegRadius), new Vector3(_bodyWidth/2, 0, -_bodyLength/2 + _upperLegRadius), Y, X, -X, Y),
            new ParentChild("RRUpper", "RRShoulder", "RRShoulder2", new Vector3(0, _upperLegLength/2, 0), new Vector3(0,0,0),Y, X, Z, -Y),
            new ParentChild("RRLower", "RRUpper", "RRElbow", new Vector3(0, _lowerLegLength/2, 0), new Vector3(0,-_upperLegLength/2, 0),Y, X, Y, X),
            new ParentChild("FLShoulder", "Hexapod", "FLShoulder", new Vector3(0, 0, _upperLegRadius), new Vector3(-_bodyWidth/2, 0, _bodyLength/2 - _upperLegRadius), Y, -X, -X, -Y),
            new ParentChild("FLUpper", "FLShoulder", "FLShoulder2", new Vector3(0, _upperLegLength/2, 0), new Vector3(0,0,0),Y, X, Z, -Y),
            new ParentChild("FLLower", "FLUpper", "FLElbow", new Vector3(0, _lowerLegLength/2, 0), new Vector3(0,-_upperLegLength/2,0),Y, X, Y, X),
            new ParentChild("MLShoulder", "Hexapod", "MLShoulder", new Vector3(0, 0, _upperLegRadius), new Vector3(-_bodyWidth/2, 0, 0), Y, -X, -X, -Y),
            new ParentChild("MLUpper", "MLShoulder", "MLShoulder2", new Vector3(0, _upperLegLength/2, 0), new Vector3(0,0,0),Y, X, Z, -Y),
            new ParentChild("MLLower", "MLUpper", "MLElbow", new Vector3(0, _lowerLegLength/2, 0), new Vector3(0,-_upperLegLength/2,0),Y, X, Y, X),
            new ParentChild("RLShoulder", "Hexapod", "RLShoulder", new Vector3(0, 0, _upperLegRadius), new Vector3(-_bodyWidth/2, 0, -_bodyLength/2 + _upperLegRadius), Y, -X, -X, -Y),
            new ParentChild("RLUpper", "RLShoulder", "RLShoulder2", new Vector3(0, _upperLegLength/2, 0), new Vector3(0,0,0),Y, X, Z, -Y),
            new ParentChild("RLLower", "RLUpper", "RLElbow", new Vector3(0, _lowerLegLength/2, 0), new Vector3(0,-_upperLegLength/2, 0),Y, X, Y, X),
        };

        string _parentName = "Hexapod";

        HexapodEntity CreateHexapod(string name, Vector3 initialPosition)
        {
            Dictionary<string, VisualEntity> hexapodShapes = new Dictionary<string, VisualEntity>();
            string prefix = name + "_";

            foreach(HexapodShapeDescriptor desc in ShapeDescriptors)
            {
                Shape newShape = null;
                switch(desc.ShapeID)
                {
                    case Shapes.Box:
                        newShape = new BoxShape(new BoxShapeProperties(
                            desc.Name + " Shape", (float)desc.mass, new Pose(), new Vector3((float)desc.xSize, (float)desc.ySize, (float)desc.zSize)));
                        break;

                    case Shapes.Capsule:
                        newShape = new CapsuleShape(new CapsuleShapeProperties(
                            desc.Name + " Shape", (float)desc.mass, new Pose(), (float)desc.radius, (float)desc.ySize));
                        break;

                    case Shapes.Sphere:
                        newShape = new SphereShape(new SphereShapeProperties(
                            desc.Name + " Shape", (float)desc.mass, new Pose(), (float)desc.radius));
                        break;
                }
                SingleShapeEntity shapeEntity = null;
                if(desc.Name == _parentName)
                {
                    shapeEntity = new HexapodEntity(newShape, new Vector3(
                        (float)desc.xPosition + initialPosition.X, 
                        (float)desc.yPosition + initialPosition.Y, 
                        (float)desc.zPosition + initialPosition.Z));
                }
                else
                {
                    shapeEntity = new SegmentEntity(newShape, new Vector3(
                        (float)desc.xPosition + initialPosition.X, 
                        (float)desc.yPosition + initialPosition.Y, 
                        (float)desc.zPosition + initialPosition.Z));
                }

                shapeEntity.State.Name = prefix + desc.Name;
                shapeEntity.State.Pose.Orientation = UIMath.EulerToQuaternion(new xna.Vector3((float)desc.xRotation, (float)desc.yRotation, (float)desc.zRotation));
                if (!string.IsNullOrEmpty(desc.mesh))
                    shapeEntity.State.Assets.Mesh = desc.mesh;
                
                hexapodShapes.Add(shapeEntity.State.Name, shapeEntity);
            }

            // now set up the Parent/Child relationships
            foreach (ParentChild rel in Relationships)
            {
                string[] names = rel.JointName.Split(';');
                JointAngularProperties angular = new JointAngularProperties();

                if ((names.Length > 0) && (names[0] != string.Empty))
                {
                    angular.TwistMode = JointDOFMode.Free;
                    angular.TwistDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 1000, 0), 1000000);
                }

                if ((names.Length > 1) && (names[1] != string.Empty))
                {
                    angular.Swing1Mode = JointDOFMode.Free;
                    angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 1000, 0), 1000000);
                }

                if ((names.Length > 2) && (names[2] != string.Empty))
                {
                    angular.Swing2Mode = JointDOFMode.Free;
                    if(angular.SwingDrive == null)
                        angular.SwingDrive = new JointDriveProperties(JointDriveMode.Position, new SpringProperties(500000, 1000, 0), 1000000);
                }

                EntityJointConnector[] connectors = new EntityJointConnector[]
                {
                    new EntityJointConnector(hexapodShapes[prefix + rel.Child], rel.ChildNormal, rel.ChildAxis, rel.ChildConnect),
                    new EntityJointConnector(hexapodShapes[prefix + rel.Parent], rel.ParentNormal, rel.ParentAxis, rel.ParentConnect)
                };
                
                SegmentEntity child = (SegmentEntity)hexapodShapes[prefix + rel.Child];
                child.CustomJoint = new Joint();
                child.CustomJoint.State = new JointProperties(angular, connectors);
                child.CustomJoint.State.Name = rel.JointName;

                hexapodShapes[prefix + rel.Parent].InsertEntityGlobal(hexapodShapes[prefix + rel.Child]);
            }

            HexapodEntity retValue = (HexapodEntity)hexapodShapes[prefix + _parentName];

            // add a camera
            AttachedCameraEntity cam = new AttachedCameraEntity();
            cam.State.Name = name + "_cam";

            // move the camera to the front of the hexapod
            cam.State.Pose = new Pose(new Vector3(0, 0, -_bodyLength / 2 + 0.02f));

            // adjust the near plane so that we can see objects that are close
            cam.Near = 0.01f;

            // use a visual model with the camera
            cam.State.Assets.Mesh = "WebCam.obj";

            // the camera coordinates are relative to the hexapod
            retValue.InsertEntity(cam);

            
            retValue.State.Flags |= EntitySimulationModifiers.Kinematic;
            retValue.State.Name = name;
            return retValue;
        }


        void AddSky()
        {
            // Add a sky using a static texture. We will use the sky texture
            // to do per pixel lighting on each simulation visual entity
            SkyDomeEntity sky = new SkyDomeEntity("skydome.dds", "sky_diff.dds");
            SimulationEngine.GlobalInstancePort.Insert(sky);
            
            // Add a directional light to simulate the sun.
            LightSourceEntity sun = new LightSourceEntity();
            sun.State.Name = "Sun";
            sun.Type = LightSourceEntityType.Directional;
            sun.Color = new Vector4(0.8f, 0.8f, 0.8f, 1);
            sun.Direction = new Vector3(0.5f, -.75f, 0.5f);
            SimulationEngine.GlobalInstancePort.Insert(sun);
        }

        private void AddGround()
        {
            TerrainEntity ground = new TerrainEntity(
                             "terrain.bmp",
                             "terrain_tex.jpg",
                             new MaterialProperties("ground",
                                 0.2f, // restitution
                                 0.5f, // dynamic friction
                                 0.5f) // static friction
                             );

            SimulationEngine.GlobalInstancePort.Insert(ground);
        }
    }

    public static class Contract
    {
        public const string Identifier = "http://www.promrds.com/contracts/2007/11/hexapodentity.html";
    }

    [ServicePort]
    public class SimulatedHexapodOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop>
    {
    }

    [DataContract]
    public class HexapodEntity : SingleShapeEntity
    {
        float shoulderAngle = 0;

        [DataMember]
        public float ShoulderAngle { get { return shoulderAngle; } set { shoulderAngle = value; } }

        float elbowAngle = 90;

        [DataMember]
        public float ElbowAngle { get { return elbowAngle; } set { elbowAngle = value; } }

        float stepAngle = -20;

        [DataMember]
        public float StepAngle { get { return stepAngle; } set { stepAngle = value; } }

        float liftAngle = -15;

        [DataMember]
        public float LiftAngle { get { return liftAngle; } set { liftAngle = value; } }

        // This class holds a description of each of the joints in the entity.
        class JointDesc
        {
            public string Name;
            public float Min;  // minimum allowable angle
            public float Max;  // maximum allowable angle
            public PhysicsJoint Joint; // Phyics Joint
            public float Target;  // Target joint position
            public float Current;  // Current joint position
            public float Speed;  // Rate of moving toward the target position
            public JointDesc(string name, float min, float max)
            {
                Name = name; Min = min; Max = max;
                Joint = null;
                Current = Target = 0;
                Speed = 30;
            }

            // Returns true if the specified target is within the valid bounds
            public bool ValidTarget(float target)
            {
                return ((target >= Min) && (target <= Max));
            }

            // Returns true if the joint is not yet at the target position
            public bool NeedToMove(float epsilon)
            {
                if (Joint == null) return false;
                return (Math.Abs(Target - Current) > epsilon);
            }

            // Takes one step toward the target position based on the specified time
            public void UpdateCurrent(double time)
            {
                float delta = (float)(time * Speed);
                if (Target > Current)
                    Current = Math.Min(Current + delta, Target);
                else
                    Current = Math.Max(Current - delta, Target);
            }
        }

        // Initialize an array of descriptions for each joint in the arm
        JointDesc[] _joints = new JointDesc[]
        {
            new JointDesc("FRShoulderFB", 40, 140),
            new JointDesc("FRShoulderUD", -90, 90),
            new JointDesc("FRElbow", -90, 90),
            new JointDesc("MRShoulderFB", 40, 140),
            new JointDesc("MRShoulderUD", -90, 90),
            new JointDesc("MRElbow", -90, 90),
            new JointDesc("RRShoulderFB", 40, 140),
            new JointDesc("RRShoulderUD", -90, 90),
            new JointDesc("RRElbow", -90, 90),
            new JointDesc("FLShoulderFB", 40, 140),
            new JointDesc("FLShoulderUD", -90, 90),
            new JointDesc("FLElbow", -90, 90),
            new JointDesc("MLShoulderFB", 40, 140),
            new JointDesc("MLShoulderUD", -90, 90),
            new JointDesc("MLElbow", -90, 90),
            new JointDesc("RLShoulderFB", 40, 140),
            new JointDesc("RLShoulderUD", -90, 90),
            new JointDesc("RLElbow", -90, 90),
        };

        /// <summary>
        /// Default constructor
        /// </summary>
        public HexapodEntity() { }

        /// <summary>
        /// Initialization constructor
        /// </summary>
        /// <param name="shape"></param>
        /// <param name="initialPos"></param>
        public HexapodEntity(Shape shape, Vector3 initialPos)
            : base(shape, initialPos)
        {
        }

        bool _moveToActive = false;
        const float _epsilon = 0.01f;
        SuccessFailurePort _moveToResponsePort = null;
        double _prevTime = 0;

        public override void Update(FrameUpdate update)
        {
            // The joint member in each joint description needs to be set after all of the joints
            // have been created.  If this has not been done yet, do it now.
            if (_joints[0].Joint == null)
            {
                int index = 0;
                foreach (VisualEntity child in Children)
                {
                    // no joints in the camera
                    if (child.GetType() == typeof(AttachedCameraEntity))
                        continue;

                    _joints[index + 0].Joint = (PhysicsJoint)child.ParentJoint;
                    _joints[index + 1].Joint = (PhysicsJoint)child.Children[0].ParentJoint;
                    _joints[index + 2].Joint = (PhysicsJoint)child.Children[0].Children[0].ParentJoint;
                    index += 3;
                }
            }

            base.Update(update);

            // update joints if necessary
            if (_moveToActive)
            {
                bool done = true;
                // Check each joint and update it if necessary.
                for (int index = 0; index < _joints.Length; index++)
                {
                    if (_joints[index].NeedToMove(_epsilon))
                    {
                        done = false;

                        Vector3 normal = _joints[index].Joint.State.Connectors[0].JointAxis;
                        _joints[index].UpdateCurrent(_prevTime);
                        _joints[index].Joint.SetAngularDriveOrientation(
                            Quaternion.FromAxisAngle(/*normal.X, normal.Y, normal.Z*/1,0,0, DegreesToRadians(_joints[index].Current)));
                    }
                }

                if (done)
                {
                    // no joints needed to be updated, the movement is finished
                    _moveToActive = false;
                    _moveToResponsePort.Post(new SuccessResult());
                }
            }
            _prevTime = update.ElapsedTime;
        }

        public SuccessFailurePort MoveTo(
            float FRShoulderFB,
            float FRShoulderUD,
            float FRElbow,
            float MRShoulderFB,
            float MRShoulderUD,
            float MRElbow,
            float RRShoulderFB,
            float RRShoulderUD,
            float RRElbow,
            float FLShoulderFB,
            float FLShoulderUD,
            float FLElbow,
            float MLShoulderFB,
            float MLShoulderUD,
            float MLElbow,
            float RLShoulderFB,
            float RLShoulderUD,
            float RLElbow,
            float time)
        {
            SuccessFailurePort responsePort = new SuccessFailurePort();

            if (_moveToActive)
            {
                responsePort.Post(new Exception("Previous MoveTo still active."));
                return responsePort;
            }

            // check bounds.  If the target is invalid, post an exception message to the response port with a helpful error.
            int index = 0;
            if (!_joints[index].ValidTarget(FRShoulderFB))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + FRShoulderFB.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(FRShoulderUD))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + FRShoulderUD.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(FRElbow))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + FRElbow.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(MRShoulderFB))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + MRShoulderFB.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(MRShoulderUD))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + MRShoulderUD.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(MRElbow))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + MRElbow.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(RRShoulderFB))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + RRShoulderFB.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(RRShoulderUD))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + RRShoulderUD.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(RRElbow))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + RRElbow.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(FLShoulderFB))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + FLShoulderFB.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(FLShoulderUD))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + FLShoulderUD.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(FLElbow))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + FLElbow.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(MLShoulderFB))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + MLShoulderFB.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(MLShoulderUD))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + MLShoulderUD.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(MLElbow))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + MLElbow.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(RLShoulderFB))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + RLShoulderFB.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(RLShoulderUD))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + RLShoulderUD.ToString()));
                return responsePort;
            }
            index++;

            if (!_joints[index].ValidTarget(RLElbow))
            {
                responsePort.Post(new Exception(_joints[index].Name + "Joint set to invalid value: " + RLElbow.ToString()));
                return responsePort;
            }
            index++;


            // set the target values on the joint descriptors
            _joints[0].Target = FRShoulderFB;
            _joints[1].Target = FRShoulderUD;
            _joints[2].Target = FRElbow;
            _joints[3].Target = MRShoulderFB;
            _joints[4].Target = MRShoulderUD;
            _joints[5].Target = MRElbow;
            _joints[6].Target = RRShoulderFB;
            _joints[7].Target = RRShoulderUD;
            _joints[8].Target = RRElbow;
            _joints[9].Target = FLShoulderFB;
            _joints[10].Target = FLShoulderUD;
            _joints[11].Target = FLElbow;
            _joints[12].Target = MLShoulderFB;
            _joints[13].Target = MLShoulderUD;
            _joints[14].Target = MLElbow;
            _joints[15].Target = RLShoulderFB;
            _joints[16].Target = RLShoulderUD;
            _joints[17].Target = RLElbow;

            // calculate a speed value for each joint that will cause it to complete its motion in the specified time
            for (int i = 0; i < _joints.Length; i++)
                _joints[i].Speed = Math.Abs(_joints[i].Target - _joints[i].Current) / time;

            // set this flag so that the motion is evaluated in the update method
            _moveToActive = true;

            // keep a pointer to the response port so we can post a result message to it.
            _moveToResponsePort = responsePort;

            return responsePort;
        }

        float DegreesToRadians(float degrees)
        {
            return (float)(degrees * Math.PI / 180);
        }
    }

    [DataContract]
    public class SegmentEntity : SingleShapeEntity
    {
        private Joint _customJoint;

        [DataMember]
        public Joint CustomJoint
        {
            get { return _customJoint; }
            set { _customJoint = value; }
        }

        /// <summary>
        /// Default constructor
        /// </summary>
        public SegmentEntity() { }

        /// <summary>
        /// Initialization constructor
        /// </summary>
        /// <param name="shape"></param>
        /// <param name="initialPos"></param>
        public SegmentEntity(Shape shape, Vector3 initialPos)
            : base(shape, initialPos)
        {
        }

        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);

            // update the parent joint to match our custom joint parameters
            if (_customJoint != null)
            {
                if (ParentJoint != null)
                    PhysicsEngine.DeleteJoint((PhysicsJoint)ParentJoint);

                // restore the entity pointers in _customJoint after deserialization if necessary
                if (_customJoint.State.Connectors[0].Entity == null)
                    _customJoint.State.Connectors[0].Entity = FindConnectedEntity(_customJoint.State.Connectors[0].EntityName, this);

                if (_customJoint.State.Connectors[1].Entity == null)
                    _customJoint.State.Connectors[1].Entity = FindConnectedEntity(_customJoint.State.Connectors[1].EntityName, this);

                ParentJoint = _customJoint;
                PhysicsEngine.InsertJoint((PhysicsJoint)ParentJoint);
            }
        }

        VisualEntity FindConnectedEntity(string name, VisualEntity me)
        {
            // find the parent at the top of the hierarchy
            while (me.Parent != null)
                me = me.Parent;

            // now traverse the hierarchy looking for the name
            return FindConnectedEntityHelper(name, me);
        }

        VisualEntity FindConnectedEntityHelper(string name, VisualEntity me)
        {
            if (me.State.Name == name)
                return me;

            foreach (VisualEntity child in me.Children)
            {
                VisualEntity result = FindConnectedEntityHelper(name, child);
                if (result != null)
                    return result;
            }

            return null;
        }

        /// <summary>
        /// Override the base PreSerialize method so that we can properly serialize joints
        /// </summary>
        public override void PreSerialize()
        {
            base.PreSerialize();
            PrepareJointsForSerialization();
        }
    }
    /// <summary>
    /// This camera model is for a camera meant to be attached as
    /// a child of another entity. It is free to move in yaw, pitch,
    /// and roll unlike the FirstPersonCamera. It ignores keyboard
    /// and mouse input.
    /// </summary>
    public class AttachedCameraModel : Camera
    {
        public VisualEntity Parent = null;
        public override void Update(double elapsedRealTime, bool hasFocus)
        {
            // no need to update this camera, the keyboard and mouse don't affect it
        }
        public override void SetViewParameters(Microsoft.Xna.Framework.Vector3 eyePt, Microsoft.Xna.Framework.Vector3 lookAtPt)
        {
            // if there is a parent set, the camera matrix is the inverse of the parent world matrix.
            if (Parent != null)
                _viewMatrix = xna.Matrix.Invert(Parent.World);
        }
    }

    [DataContract]
    public class AttachedCameraEntity : CameraEntity
    {
        public AttachedCameraEntity()
            : base()
        {
        }

        public override void Initialize(Microsoft.Xna.Framework.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);

            // replace the camera model used by this camera entity using reflection
            System.Reflection.FieldInfo fInfo = GetType().BaseType.GetField("_frameworkCamera",
                System.Reflection.BindingFlags.Instance |
                System.Reflection.BindingFlags.NonPublic);
            if (fInfo != null)
            {
                AttachedCameraModel newCamera = new AttachedCameraModel();
                newCamera.Parent = this;
                fInfo.SetValue(this, newCamera);
            }

            SetProjectionParameters(
                ViewAngle,
                ((float)ViewSizeX / (float)ViewSizeY),
                Near,
                Far,
                ViewSizeX,
                ViewSizeY);
        }
    }
}
